﻿using ConsoleApp;
using OSGeo.GDAL;
using OSGeo.OGR;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;

namespace chatgpt
{
    public class UAVGeoCalculator
    {
        // WGS84参数
        const double a = 6378137.0;
        const double e2 = 0.00669437999014;
        public static void LMain()
        {
            // GDAL初始化
            GdalConfiguration.ConfigureGdal();
            GdalConfiguration.ConfigureOgr();

            Gdal.AllRegister();
            Ogr.RegisterAll();

            string dsmPath = @"D:\data\PowerStations\测试\LandformFile\dsm_yuanhang.tif"; // TODO: 修改为你的DSM文件路径
            Dataset dsmDataset = Gdal.Open(dsmPath, Access.GA_ReadOnly);
            if (dsmDataset == null)
            {
                Console.WriteLine("无法打开DSM文件");
                return;
            }

            // 无人机姿态角（度）
            double droneYaw = 88.9;
            double dronePitch = -18.4;
            double droneRoll = 15.0;

            // 云台姿态角（度）
            double gimbalYaw = -89.3;
            double gimbalPitch = -90.0;
            double gimbalRoll = 180.0;

            // 无人机地理位置和高度
            double droneLat = 36.003380555555559;
            double droneLon = 100.20005;
            double droneAltitude = 3109.71;

            // 相机参数
            int imgWidth = 640;
            int imgHeight = 512;
            double focalLengthMm = 24.0;
            double pixelSizeUm = 12.0;

            // 目标框像素中心
            double targetX = (169 + 192) / 2.0;
            double targetY = (253 + 296) / 2.0;

            // 图像中心像素
            double centerX = imgWidth / 2.0;
            double centerY = imgHeight / 2.0;

            // 无人机ECEF坐标
            Vector3d droneECEF = GeodeticToECEF(droneLat, droneLon, droneAltitude);

            // 计算旋转矩阵
            Matrix3x3 R_drone = EulerAnglesToRotationMatrix(droneYaw, dronePitch, droneRoll);
            Matrix3x3 R_gimbal = EulerAnglesToRotationMatrix(gimbalYaw, gimbalPitch, gimbalRoll);
            Matrix3x3 R = Matrix3x3.Multiply(R_drone, R_gimbal);

            // 计算相机视线方向（相机坐标系）
            Vector3d dirCenterCam = PixelToCameraDirection(centerX, centerY, imgWidth, imgHeight, focalLengthMm, pixelSizeUm);
            Vector3d dirTargetCam = PixelToCameraDirection(targetX, targetY, imgWidth, imgHeight, focalLengthMm, pixelSizeUm);

            // 旋转到世界坐标系
            Vector3d dirCenterWorld = R * dirCenterCam;
            Vector3d dirTargetWorld = R * dirTargetCam;

            // 归一化方向向量
            dirCenterWorld = Vector3d.Normalize(dirCenterWorld);
            dirTargetWorld = Vector3d.Normalize(dirTargetWorld);

            // 射线步长和最大距离（米）
            double stepSize = 1.0;
            double maxDistance = 5000.0;

            // 射线搜索DSM地面交点（经纬度+高度）
            var centerGeo = RaycastToDSM(droneECEF, dirCenterWorld, dsmDataset, stepSize, maxDistance);
            var targetGeo = RaycastToDSM(droneECEF, dirTargetWorld, dsmDataset, stepSize, maxDistance);

            Console.WriteLine("=== 计算结果 ===");
            Console.WriteLine($"图像中心经纬度：纬度 {centerGeo.lat:F8}，经度 {centerGeo.lon:F8}，高程 {centerGeo.height:F2} 米");
            Console.WriteLine($"目标物经纬度：纬度 {targetGeo.lat:F8}，经度 {targetGeo.lon:F8}，高程 {targetGeo.height:F2} 米");

            dsmDataset.Dispose();

            Console.WriteLine("按任意键退出...");
            Console.ReadKey();
        }
        public static void LMain1()
        {
            // GDAL初始化
            GdalConfiguration.ConfigureGdal();
            GdalConfiguration.ConfigureOgr();

            Gdal.AllRegister();
            Ogr.RegisterAll();

            string dsmPath = @"D:\data\PowerStations\国家光伏储能实证实验平台大庆基地一期\LandformFile\dsm.tif"; // TODO: 修改为你的DSM文件路径
            Dataset dsmDataset = Gdal.Open(dsmPath, Access.GA_ReadOnly);
            if (dsmDataset == null)
            {
                Console.WriteLine("无法打开DSM文件");
                return;
            }

            // 无人机姿态角（度）
            double droneYaw = 0;
            double dronePitch = -3.6;
            double droneRoll = -1.9;

            // 云台姿态角（度）
            double gimbalYaw = -0;
            double gimbalPitch = -45.0;
            double gimbalRoll = 0;

            // 无人机地理位置和高度
            double droneLat = 46.147141666666663;
            double droneLon = 124.87156111111111;
            double droneAltitude = 192.37;

            // 相机参数
            int imgWidth = 1280;
            int imgHeight = 1024;
            double focalLengthMm = 24.0;
            double pixelSizeUm = 12.0;

            // 目标框像素中心
            double targetX = (287 + 346) / 2.0;
            double targetY = (354 + 588) / 2.0;

            // 图像中心像素
            double centerX = imgWidth / 2.0;
            double centerY = imgHeight / 2.0;

            // 无人机ECEF坐标
            Vector3d droneECEF = GeodeticToECEF(droneLat, droneLon, droneAltitude);

            // 计算旋转矩阵
            Matrix3x3 R_drone = EulerAnglesToRotationMatrix(droneYaw, dronePitch, droneRoll);
            Matrix3x3 R_gimbal = EulerAnglesToRotationMatrix(gimbalYaw, gimbalPitch, gimbalRoll);
            Matrix3x3 R = Matrix3x3.Multiply(R_drone, R_gimbal);

            // 计算相机视线方向（相机坐标系）
            Vector3d dirCenterCam = PixelToCameraDirection(centerX, centerY, imgWidth, imgHeight, focalLengthMm, pixelSizeUm);
            Vector3d dirTargetCam = PixelToCameraDirection(targetX, targetY, imgWidth, imgHeight, focalLengthMm, pixelSizeUm);

            // 旋转到世界坐标系
            Vector3d dirCenterWorld = R * dirCenterCam;
            Vector3d dirTargetWorld = R * dirTargetCam;

            // 归一化方向向量
            dirCenterWorld = Vector3d.Normalize(dirCenterWorld);
            dirTargetWorld = Vector3d.Normalize(dirTargetWorld);

            // 射线步长和最大距离（米）
            double stepSize = 1.0;
            double maxDistance = 5000.0;

            // 射线搜索DSM地面交点（经纬度+高度）
            var centerGeo = RaycastToDSM(droneECEF, dirCenterWorld, dsmDataset, stepSize, maxDistance);
            var targetGeo = RaycastToDSM(droneECEF, dirTargetWorld, dsmDataset, stepSize, maxDistance);

            Console.WriteLine("=== 计算结果 ===");
            Console.WriteLine($"图像中心经纬度：纬度 {centerGeo.lat:F8}，经度 {centerGeo.lon:F8}，高程 {centerGeo.height:F2} 米");
            Console.WriteLine($"目标物经纬度：纬度 {targetGeo.lat:F8}，经度 {targetGeo.lon:F8}，高程 {targetGeo.height:F2} 米");

            dsmDataset.Dispose();

            Console.WriteLine("按任意键退出...");
            Console.ReadKey();
        }
        #region 结构体及辅助函数

        // 三维双精度向量
        public struct Vector3d
        {
            public double X, Y, Z;
            public Vector3d(double x, double y, double z) { X = x; Y = y; Z = z; }

            public static Vector3d operator +(Vector3d a, Vector3d b)
                => new Vector3d(a.X + b.X, a.Y + b.Y, a.Z + b.Z);

            public static Vector3d operator -(Vector3d a, Vector3d b)
                => new Vector3d(a.X - b.X, a.Y - b.Y, a.Z - b.Z);

            public static Vector3d operator *(Vector3d v, double s)
                => new Vector3d(v.X * s, v.Y * s, v.Z * s);

            public static Vector3d operator *(Matrix3x3 m, Vector3d v)
                => new Vector3d(
                    m.M11 * v.X + m.M12 * v.Y + m.M13 * v.Z,
                    m.M21 * v.X + m.M22 * v.Y + m.M23 * v.Z,
                    m.M31 * v.X + m.M32 * v.Y + m.M33 * v.Z);

            public static Vector3d Normalize(Vector3d v)
            {
                double len = Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
                return new Vector3d(v.X / len, v.Y / len, v.Z / len);
            }
        }

        // 3x3 矩阵
        public struct Matrix3x3
        {
            public double M11, M12, M13;
            public double M21, M22, M23;
            public double M31, M32, M33;

            public static Matrix3x3 Multiply(Matrix3x3 a, Matrix3x3 b)
            {
                return new Matrix3x3
                {
                    M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31,
                    M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32,
                    M13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33,

                    M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31,
                    M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32,
                    M23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33,

                    M31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31,
                    M32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32,
                    M33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33
                };
            }
        }

        // 角度转弧度
        static double Deg2Rad(double deg) => deg * Math.PI / 180.0;

        // 姿态角转旋转矩阵，旋转顺序 Yaw(Z) - Pitch(Y) - Roll(X)
        static Matrix3x3 EulerAnglesToRotationMatrix(double yaw, double pitch, double roll)
        {
            double cy = Math.Cos(Deg2Rad(yaw));
            double sy = Math.Sin(Deg2Rad(yaw));
            double cp = Math.Cos(Deg2Rad(pitch));
            double sp = Math.Sin(Deg2Rad(pitch));
            double cr = Math.Cos(Deg2Rad(roll));
            double sr = Math.Sin(Deg2Rad(roll));

            return new Matrix3x3
            {
                M11 = cy * cp,
                M12 = cy * sp * sr - sy * cr,
                M13 = cy * sp * cr + sy * sr,
                M21 = sy * cp,
                M22 = sy * sp * sr + cy * cr,
                M23 = sy * sp * cr - cy * sr,
                M31 = -sp,
                M32 = cp * sr,
                M33 = cp * cr
            };
        }

        // WGS84经纬度+高度转ECEF坐标
        static Vector3d GeodeticToECEF(double latDeg, double lonDeg, double h)
        {
            double lat = Deg2Rad(latDeg);
            double lon = Deg2Rad(lonDeg);

            double N = a / Math.Sqrt(1 - e2 * Math.Sin(lat) * Math.Sin(lat));
            double x = (N + h) * Math.Cos(lat) * Math.Cos(lon);
            double y = (N + h) * Math.Cos(lat) * Math.Sin(lon);
            double z = (N * (1 - e2) + h) * Math.Sin(lat);

            return new Vector3d(x, y, z);
        }

        // ECEF坐标转WGS84经纬度+高度
        static (double lat, double lon, double h) ECEFToGeodetic(Vector3d ecef)
        {
            double x = ecef.X;
            double y = ecef.Y;
            double z = ecef.Z;

            double lon = Math.Atan2(y, x);
            double p = Math.Sqrt(x * x + y * y);
            double lat = Math.Atan2(z, p * (1 - e2));
            double latPrev;
            double h = 0;

            for (int i = 0; i < 5; i++)
            {
                latPrev = lat;
                double N = a / Math.Sqrt(1 - e2 * Math.Sin(lat) * Math.Sin(lat));
                h = p / Math.Cos(lat) - N;
                lat = Math.Atan2(z, p * (1 - e2 * (N / (N + h))));
                if (Math.Abs(lat - latPrev) < 1e-12) break;
            }

            return (lat * 180.0 / Math.PI, lon * 180.0 / Math.PI, h);
        }

        // 像素坐标转相机坐标系方向向量（单位向量）
        static Vector3d PixelToCameraDirection(double px, double py, int imgWidth, int imgHeight, double focalLengthMm, double pixelSizeUm)
        {
            double cx = imgWidth / 2.0;
            double cy = imgHeight / 2.0;

            double x = (px - cx) * pixelSizeUm * 1e-6;  // 米
            double y = (cy - py) * pixelSizeUm * 1e-6;  // 米，注意y轴正方向
            double z = focalLengthMm * 1e-3;            // 米

            Vector3d dir = new Vector3d(x, y, z);
            return Vector3d.Normalize(dir);
        }

        // 经纬度转栅格坐标（行列）
        static void GeoToPixel(double[] geoTransform, double lon, double lat, out int pixel, out int line)
        {
            double originX = geoTransform[0];
            double pixelSizeX = geoTransform[1];
            double rotX = geoTransform[2];
            double originY = geoTransform[3];
            double rotY = geoTransform[4];
            double pixelSizeY = geoTransform[5];

            double det = pixelSizeX * pixelSizeY - rotX * rotY;
            pixel = (int)((pixelSizeY * (lon - originX) - rotX * (lat - originY)) / det);
            line = (int)((-rotY * (lon - originX) + pixelSizeX * (lat - originY)) / det);
        }

        // 沿射线从origin出发方向dir搜索DSM地面交点，步长step，最大距离maxDist，返回经纬度和高程
        static (double lat, double lon, double height) RaycastToDSM(Vector3d origin, Vector3d dir, Dataset dsm, double step, double maxDist)
        {
            double[] geoTransform = new double[6];
            dsm.GetGeoTransform(geoTransform);

            Band band = dsm.GetRasterBand(1);

            for (double dist = 0; dist < maxDist; dist += step)
            {
                Vector3d pos = origin + dir * dist;
                var geo = ECEFToGeodetic(pos);

                int px, py;
                GeoToPixel(geoTransform, geo.lon, geo.lat, out px, out py);

                if (px < 0 || px >= dsm.RasterXSize || py < 0 || py >= dsm.RasterYSize)
                    continue;

                float[] heightBuf = new float[1];
                band.ReadRaster(px, py, 1, 1, heightBuf, 1, 1, 0, 0);
                double dsmHeight = heightBuf[0];

                if (pos.Z <= dsmHeight + 0.5) // 0.5米容差
                    return (geo.lat, geo.lon, dsmHeight);
            }

            // 未找到交点，返回origin的经纬度
            return ECEFToGeodetic(origin);
        }

        #endregion
    }
}
